"""
这个脚本是基于预标定结果自动找到点云中四点
雷达产生的csv文件请命名为{number}_cols.csv
由于会出现排布问题，请手动输入排布，以rd,ld,lt,rt为0,1,2,3
"""
from camera_utils.camera import read_yaml
from camera_utils.config import CALIB_DIR,CALIB_MODE,CAMERA_TYPE
import numpy as np
import cv2

# 预标定结果
T = np.float64([0.161811409, -0.986745049, 0.012299438, 0.090505390,
-0.011824027, -0.014401456, -0.999826380, -0.001785240,
0.986750860, 0.161637886, -0.013997620, 0.014308105,
0.000000000, 0.000000000, 0.000000000, 1.000000000,]).reshape(4,4)

def find_corner():
    # two camera
    dir = CALIB_DIR
    ct = 0
    num = 7
    if CALIB_MODE == 1:
        ct = CAMERA_TYPE
    _,K_0,C_0,_,_ = read_yaml(ct)
    for i in range(num):
        df = np.loadtxt(f"{dir}/{i}_cols.csv",  delimiter=' ')
        df = df[:,:3].astype(np.float32)

        norm = np.linalg.norm(df,axis = 1)

        df = df[norm > 0.4]

        ips = cv2.projectPoints(df, cv2.Rodrigues(T[:3, :3])[0], T[:3, 3], K_0, C_0)[0].reshape(-1, 2)


        rect = cv2.minAreaRect(ips.reshape(-1,1,2))

        ps = cv2.boxPoints(rect).reshape(-1,2)



        box = []

        cv2.namedWindow("out",cv2.WINDOW_NORMAL)
        if CALIB_MODE == 1:
            im = cv2.imread(f"{dir}/{ct}_c{i}.jpg")
        else:
            im = cv2.imread(f"{dir}/{ct}_c{i*2+ct}.jpg")

        for j in range(4):
            cv2.circle(im,tuple(ps[j].astype(int)),7,(0,255,0),-1)
            cv2.imshow("out",im)
            cv2.waitKey(0)
            mul = np.sum((ips.reshape(-1,2)-ps[j].reshape(1,2))**2,axis = 1)
            box.append(np.argmin(mul))

        cn = df[box]
        path = f"{dir}/{ct}_{i*2+ct}_cors.txt" if CALIB_MODE==2 else f"{dir}/{ct}_{i}_cors.txt"

        cn_copy = cn.copy()
        point_order = input(
            "Please input order:")
        if len(point_order):
            assert len(point_order) == 4
            for i in range(4):
                cn[i] = cn_copy[point_order.find(str(i))]

        with open(path,'w') as f:
            for p in cn:
                f.write("%.9f %.9f %.9f\n"%tuple(p))

if __name__ == '__main__':
    find_corner()